// Bounding interval hierarchy

#include "pathological.h"


#define LEAF_SIZE 1


typedef enum {
	BIH_X_NODE = 0,
	BIH_Y_NODE = 1,
	BIH_Z_NODE = 2,
	BIH_LEAF
} bih_node_type_t;

typedef struct bih_node_t bih_node_t;
struct bih_node_t {
	bih_node_type_t type;
	union {
		struct {
			float clips[2];
			bih_node_t *children;
		} xyz;
		struct {
			object_t **objects;
			int numobjects;
		} leaf;
	} data;
} PACKED;


bih_node_t *bihnodes;
aabb_t *objaabbs, sceneaabb;


void swap_objects(int a, int b)
{
	object_t *tempobj = *(objects + a);
	objects[a] = objects[b];
	*(objects + b) = tempobj;
	aabb_t tempaabb = objaabbs[a];
	objaabbs[a] = objaabbs[b];
	objaabbs[b] = tempaabb;
}

// Returns updated freenodes pointer
bih_node_t *bih_add_node(bih_node_t *node, int startobj, int numobj,
	aabb_t *nodeaabb, bih_node_t *freenodes)
{
	if(numobj <= LEAF_SIZE) // Leaf node?
	{
leaf_node:
		node->type = BIH_LEAF;
		node->data.leaf.objects = objects + startobj;
		node->data.leaf.numobjects = numobj;
		return freenodes;
	}

	aabb_t childaabb = *nodeaabb;
	int axes[3], axis, i, li, ri, try;
	float xsize = nodeaabb->max[0] - nodeaabb->min[0],
		ysize = nodeaabb->max[1] - nodeaabb->min[1],
		zsize = nodeaabb->max[2] - nodeaabb->min[2],
		mid;

	axes[(xsize <= ysize) + (xsize < zsize)] = 0;
	axes[(ysize < xsize) + (ysize <= zsize)] = 1;
	axes[(zsize <= xsize) + (zsize < ysize)] = 2;

	for(try = 0; try < 3; try++)
	{
		li = startobj;
		ri = startobj + numobj;
		axis = axes[try];
		node->type = BIH_X_NODE;
		mid = (nodeaabb->min[axis] + nodeaabb->max[axis])/2;

		while(li < ri)
			if((objaabbs[li].min[axis] + objaabbs[li].max[axis])/2 < mid) li++;
			else swap_objects(li,--ri);

		if(li > startobj && ri < startobj + numobj) break;
	}
	if(try >= 3) goto leaf_node; // Give up

	node->type = axis;

	// Fit the clips
	for(node->data.xyz.clips[0] = -FLT_MAX, i = startobj; i < li; i++)
		node->data.xyz.clips[0]
			= MAX(node->data.xyz.clips[0],objaabbs[i].max[axis]);
	for(node->data.xyz.clips[1] = FLT_MAX; i < startobj + numobj; i++)
		node->data.xyz.clips[1]
			= MIN(node->data.xyz.clips[1],objaabbs[i].min[axis]);

	node->data.xyz.children = freenodes;

	// Think of the children!
	childaabb.max[axis] = node->data.xyz.clips[0];
	freenodes = bih_add_node(node->data.xyz.children,startobj,li - startobj,
		&childaabb,freenodes + 2);
	childaabb.max[axis] = nodeaabb->max[axis];
	childaabb.min[axis] = node->data.xyz.clips[1];
	return bih_add_node(node->data.xyz.children + 1,li,
		numobj - (li - startobj),&childaabb,freenodes);
}

void bih_build()
{
	int i;

	printf("Building bounding interval hierarchy...");

	assert(objaabbs = calloc(numobjects,sizeof(aabb_t)));
	assert(bihnodes = calloc(2*numobjects - 1,sizeof(bih_node_t)));

	for(i = 0; i < numobjects; i++)
		objects[i]->aabb(objaabbs + i,objects[i]),
		sceneaabb.min[0] = MIN(sceneaabb.min[0],objaabbs[i].min[0]),
		sceneaabb.min[1] = MIN(sceneaabb.min[1],objaabbs[i].min[1]),
		sceneaabb.min[2] = MIN(sceneaabb.min[2],objaabbs[i].min[2]),
		sceneaabb.max[0] = MAX(sceneaabb.max[0],objaabbs[i].max[0]),
		sceneaabb.max[1] = MAX(sceneaabb.max[1],objaabbs[i].max[1]),
		sceneaabb.max[2] = MAX(sceneaabb.max[2],objaabbs[i].max[2]);

	bih_add_node(bihnodes,0,numobjects,&sceneaabb,bihnodes + 1);

	free(objaabbs);

	printf("done.\n");
}

inline void bih_intersect_node(ray_t *ray, point_t *point, object_t **object,
	bih_node_t *node, aabb_t *aabb)
{
	int i;
	point_t p;
	aabb_t caabb = *aabb;
	double tx, ty, tz, t1, t2, coord1, coord2, raymin[3], raymax[3];

	if(node->type == BIH_LEAF)
	{
		for(i = 0; i < node->data.leaf.numobjects; i++)
		{
			node->data.leaf.objects[i]->intersect(node->data.leaf.objects[i],
				ray,&p);
			if(EPSILON < p.t && p.t < point->t)
				*object = node->data.leaf.objects[i], *point = p;
		}
		return;
	}

	if(ray->d.x > 0) raymin[0] = aabb->min[0], raymax[0] = aabb->max[0];
	else raymin[0] = aabb->max[0], raymax[0] = aabb->min[0];
	if(ray->d.y > 0) raymin[1] = aabb->min[1], raymax[1] = aabb->max[1];
	else raymin[1] = aabb->max[1], raymax[1] = aabb->min[1];
	if(ray->d.z > 0) raymin[2] = aabb->min[2], raymax[2] = aabb->max[2];
	else raymin[2] = aabb->max[2], raymax[2] = aabb->min[2];

	tx = (raymin[0] - ray->o.x)/ray->d.x,
	ty = (raymin[1] - ray->o.y)/ray->d.y,
	tz = (raymin[2] - ray->o.z)/ray->d.z;
	if((t1 = MAX(tx,MAX(ty,tz))) < 0) t1 = 0;

	tx = (raymax[0] - ray->o.x)/ray->d.x,
	ty = (raymax[1] - ray->o.y)/ray->d.y,
	tz = (raymax[2] - ray->o.z)/ray->d.z;
	if((t2 = MIN(tx,MIN(ty,tz))) < 0) t2 = 0;

	if(node->type == BIH_X_NODE)
		coord1 = ray->o.x + t1*ray->d.x, coord2 = ray->o.x + t2*ray->d.x;
	else if(node->type == BIH_Y_NODE)
		coord1 = ray->o.y + t1*ray->d.y, coord2 = ray->o.y + t2*ray->d.y;
	else coord1 = ray->o.z + t1*ray->d.z, coord2 = ray->o.z + t2*ray->d.z;

	if(coord1 <= node->data.xyz.clips[0] + EPSILON
		|| coord2 <= node->data.xyz.clips[0] + EPSILON)
		caabb.max[node->type] = node->data.xyz.clips[0],
		bih_intersect_node(ray,point,object,node->data.xyz.children,&caabb),
		caabb.max[node->type] = aabb->max[node->type];

	if(coord1 >= node->data.xyz.clips[1] - EPSILON
		|| coord2 >= node->data.xyz.clips[1] - EPSILON)
		caabb.min[node->type] = node->data.xyz.clips[1],
		bih_intersect_node(ray,point,object,node->data.xyz.children + 1,
			&caabb);
}

// Returns true on intersect, false otherwise
inline int intersect_ray(ray_t *ray, point_t *point, object_t **object)
{
	point->t = DBL_MAX;
	bih_intersect_node(ray,point,object,bihnodes,&sceneaabb);
	return point->t != DBL_MAX;
}

